



#include <stdio.h>
#include <stdlib.h>
#include <cstdlib>
#include <errno.h>
#include <string>
#include <sstream>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <unistd.h>
#include <stdlib.h>
#include <fcntl.h>
#include <time.h>
#include <string>
#include <sys/time.h>
#include <time.h>
#include <arpa/inet.h>
#include <stdbool.h> /* required for the definition of bool in C99 */
#include <common/mavlink.h>



#define BUFFER_LENGTH 1024
#define LOCAL_PORT  14551   //本地监听(server端)  14551
#define MAV_PORT  	14540   //onboard 
#define QGC_PORT	14550	//QGC STATION

#define MAV_ID	1
#define QGC_ID  255


uint64_t microsSinceEpoch();

int main(int argc, char* argv[])
{

	// Check if --help flag was used
	char help[] = "--help";
	if ((argc == 2) && (strcmp(argv[1], help) == 0))
    {
		printf("\n");
		printf("\tUsage:\n\n");
		printf("\t");
		printf("%s", argv[0]);
		printf(" <ip address of QGroundControl>\n");
		printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
		exit(EXIT_FAILURE);
    }

//the QGC station ip
  char target_ip[20];
  strcpy(target_ip, "127.0.0.1");
	if (argc == 2)
    {
		strcpy(target_ip, argv[1]);
    }


//creat the local server socket　－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
	struct sockaddr_in locAddr;
	memset(&locAddr, 0, sizeof(locAddr));
	locAddr.sin_family = AF_INET;
	locAddr.sin_addr.s_addr = INADDR_ANY;
	locAddr.sin_port = htons(LOCAL_PORT);	

	int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);

	/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ 
	if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
    {
		perror("error bind failed");
		close(sock);
		exit(EXIT_FAILURE);
    } 
	

	if (fcntl(sock, F_SETFL, O_NONBLOCK | O_ASYNC) < 0)

    {
		fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
		close(sock);
		exit(EXIT_FAILURE);
    }
//	-----------------------------------------------------------------------------------------------

// onboard px4----------------------------------------------------------------------
	uint8_t px4_buf[BUFFER_LENGTH];
	ssize_t px4_recsize;
	struct sockaddr_in px4Addr; //on board px4
	socklen_t px4len;
	char px4_ip[INET_ADDRSTRLEN] = "";//INET_ADDRSTRLEN=16  

// qground conrol station----------------------------------------------------------------------
	uint8_t qgc_buf[BUFFER_LENGTH];
	ssize_t qgc_recsize;
	struct sockaddr_in qgcAddr; //on board px4
	socklen_t qgclen;
	char qgc_ip[INET_ADDRSTRLEN] = "127.0.0.1";//INET_ADDRSTRLEN=16  

// received address message ---------------------------------------------------------------------

	uint8_t rec_buf[BUFFER_LENGTH];
	ssize_t rec_recsize;
	struct sockaddr_in recAddr; //on board px4
	char rec_ip[INET_ADDRSTRLEN] = "";//INET_ADDRSTRLEN=16 
	socklen_t reclen;

	std::string port;
//        std::stringstream port_temp;

//        mavlink_message_t ap_msg;
    uint8_t send_buf[1024];
	char ap_port[10];
	uint8_t ap_len;
	uint8_t bytes_sent;
	printf("\r\n\r\n the mavlink_message transmiting system is running \r\n\r\n ");
	for(;;)
	{

		memset(rec_buf, 0, BUFFER_LENGTH);
		rec_recsize = recvfrom(sock, (void *)rec_buf, sizeof(rec_buf),0, (struct sockaddr *)&recAddr, &reclen);	

		if (rec_recsize > 0)
      	{
		 	mavlink_message_t msg;
			mavlink_status_t status;
			mavlink_message_t send_msg;
		  //打印 px4 -mavros ip
		  inet_ntop(AF_INET, &recAddr.sin_addr, rec_ip, INET_ADDRSTRLEN);

			unsigned int temp = 0;
			printf("Bytes Received: %d\nDatagram: ", (int)rec_recsize);  //received bytes
			for (char i = 0; i < rec_recsize; ++i)
			{
				temp = rec_buf[i];
                //收到的数据
				printf("%02x ", (unsigned char)temp);
                //数据解析
				if (mavlink_parse_char(MAVLINK_COMM_0, rec_buf[i], &msg, &status))
				{
					// Packet received
					printf("\nReceived packet: SYSID: %d, COMPID: %d, LEN: %d,\r\nMSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
				}
			}
			printf("\n"); 
		  
		  //-----rec MAVROS ----------
		  if(msg.sysid == MAV_ID)
		  {
			//px4_ip = rec_ip;
			px4Addr = recAddr;
		 	printf("\n mavros ip:%s,   port:%d\n",rec_ip, ntohs(px4Addr.sin_port));// 客户端的ip
			//send mavros massage to QGC
			sendto(sock, rec_buf, rec_recsize, 0, (struct sockaddr*)&qgcAddr, reclen); 
			 
		  }

		  else if(msg.sysid == QGC_ID)
		  {
			//qgc_ip = rec_ip;
			qgcAddr = recAddr;
			sendto(sock, rec_buf, rec_recsize, 0, (struct sockaddr*)&px4Addr, reclen); 
		        printf("\n QGC ip:%s,   port:%d\n",rec_ip, ntohs(qgcAddr.sin_port));// 客户端的ip


			/*Send message */
		mavlink_msg_wifi_config_ap_pack(2, 200, &send_msg, "192.168.0.1", "57566");
		memset(send_buf, 0, BUFFER_LENGTH);
		ap_len = mavlink_msg_to_send_buffer(send_buf, &send_msg);
		bytes_sent = sendto(sock, send_buf, ap_len, 0, (struct sockaddr*)&px4Addr, sizeof(struct sockaddr_in));
		printf("\n send %d bytes message to mavros\n\n",bytes_sent);

		

			// ust mavlink_message_v2_299 send QGC ip&port
		//itoa(ntohs(qgcAddr.sin_port), ap_port, 10);
//		sprintf(ap_port,"%d", ntohs(qgcAddr.sin_port));
		//mavlink_msg_wifi_config_ap_pack(255, 1, &ap_msg, rec_ip, ap_port);
//		mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
//		mavlink_msg_to_send_buffer(ap_buf, &ap_msg);
		//sendto(sock, ap_buf, ap_len, 0, (struct sockaddr*)&px4Addr, ap_len);
		//printf("\r\n send the QGC IP&PORT to onboard computer %s  \r\n",ap_buf);
		  }

		  else printf("\r\n SYSTEM ID :%d is error " ,msg.sysid);
		  memset(rec_buf, 0, BUFFER_LENGTH);

		}	

		sleep(0.1);
	}
	
	close(sock);
}
